#include "filter/map_filter_impl.h"

#include <iostream>

#include "common/utility.h"
#include "generated/version.h"

namespace autodrive {
namespace perception {
MapFilterImpl::MapFilterImpl()
    : is_init_{false},
      history_map_ptr_{nullptr},
      history_map_tmp_ptr_{nullptr},
      history_pose_{Eigen::Affine3d::Identity()},
      fusion_result_ptr_{nullptr},
      road_edge_predict_ptr_{nullptr},
      lane_divier_predict_ptr_{nullptr},
      center_line_predict_ptr_{nullptr} {}

MapFilterImpl::~MapFilterImpl() {}

void MapFilterImpl::Init(MapFilter::MapFilterParameter const &para) noexcept(
    false) {
  // parameter check
  float const &weightLower{0};
  float const &weightUpper{1};

  if (para.fusion_parameter.history_weight < weightLower ||
      para.fusion_parameter.history_weight > weightUpper) {
    throw std::invalid_argument(
        "fusion parameter history_weight out of range: " +
        std::to_string(para.fusion_parameter.history_weight));
  }

  parameter_ = para;
  auto initFun{[&]() {
    if (history_map_ptr_ == nullptr) {
      history_map_ptr_ = std::make_shared<Map>();
    }
    if (history_map_tmp_ptr_ == nullptr) {
      history_map_tmp_ptr_ = std::make_shared<Map>();
    }
    if (road_edge_predict_ptr_ == nullptr) {
      road_edge_predict_ptr_ = std::make_shared<MapElementSet<RoadEdge>>();
    }
    if (lane_divier_predict_ptr_ == nullptr) {
      lane_divier_predict_ptr_ = std::make_shared<MapElementSet<LaneDivider>>();
    }
    if (center_line_predict_ptr_ == nullptr) {
      center_line_predict_ptr_ = std::make_shared<MapElementSet<CenterLine>>();
    }
    if (fusion_result_ptr_ == nullptr) {
      fusion_result_ptr_ = std::make_shared<MapElementPointSet>();
    }

    show_info();
  }};
  std::call_once(init_flag_, initFun);
  is_init_ = true;
}

void MapFilterImpl::show_info() noexcept {
  std::cout << "************************************" << std::endl;
  std::cout << "*         Map Filter Module        *" << std::endl;
  std::cout << "*                                  *" << std::endl;
  std::cout << "* Version: " << PROJECT_VERSION << "                  *"
            << std::endl;
  std::cout << "*                                  *" << std::endl;
  std::cout << "* Compile time: " << COMPILE_TIME << "   *" << std::endl;
  std::cout << "*                                  *" << std::endl;
  std::cout << "************************************" << std::endl;
}

Eigen::Vector3d MapFilterImpl::predict(
    Eigen::Vector3d const &src, Eigen::Affine3d const &history_pose,
    Eigen::Affine3d const &current_pose) noexcept(false) {
  bool isInversable{false};
  Eigen::Affine3d currenPoseInv;
  currenPoseInv.matrix() = Eigen::Matrix4d::Zero();
  current_pose.matrix().computeInverseWithCheck(currenPoseInv.matrix(),
                                                isInversable);
  if (!isInversable) {
    throw std::runtime_error("current_pose is inversable");
  }
  return currenPoseInv * history_pose * src;
}

std::pair<MapFilterImpl::OverlapDomain, MapFilterImpl::OverlapDomain>
MapFilterImpl::get_overlap_domain(
    std::vector<PointDistanceItem> const &p1_to_p2_min_dist_list,
    std::vector<PointDistanceItem> const &p2_to_p1_min_dist_list) noexcept {
  auto const &p1PointNumber{p1_to_p2_min_dist_list.size()};
  auto const &p2PointNumber{p2_to_p1_min_dist_list.size()};

  OverlapDomain p1OverlapDomain{
      .start_index{0},
      .stop_index{static_cast<std::uint32_t>(p1PointNumber - 1)}};

  OverlapDomain p2OverlapDomain{
      .start_index{0},
      .stop_index{static_cast<std::uint32_t>(p2PointNumber - 1)}};

  // calculate p1 start index
  if (p1_to_p2_min_dist_list.front().dst_index == 0 &&
      p2_to_p1_min_dist_list.front().dst_index != 0) {
    p1OverlapDomain.start_index = p2_to_p1_min_dist_list.front().dst_index;
  }

  // calculate p1 stop index
  if (p1_to_p2_min_dist_list.back().dst_index == p2PointNumber - 1 &&
      p2_to_p1_min_dist_list.back().dst_index < p1PointNumber - 1) {
    p1OverlapDomain.stop_index = p2_to_p1_min_dist_list.back().dst_index;
  }

  // calculate p2 start index
  if (p2_to_p1_min_dist_list.front().dst_index == 0 &&
      p1_to_p2_min_dist_list.front().dst_index != 0) {
    p2OverlapDomain.start_index = p1_to_p2_min_dist_list.front().dst_index;
  }

  // calculate p2 stop index
  if (p2_to_p1_min_dist_list.back().dst_index == p1PointNumber - 1 &&
      p1_to_p2_min_dist_list.back().dst_index < p2PointNumber - 1) {
    p2OverlapDomain.stop_index = p1_to_p2_min_dist_list.back().dst_index;
  }
  return std::make_pair(p1OverlapDomain, p2OverlapDomain);
}

void MapFilterImpl::fusion_match_points(
    MapFilter::FusionParamter const &parameter,
    MatchItem &matchItem) noexcept(false) {
  if (matchItem.predict_map_points_element_ptr == nullptr) {
    throw std::runtime_error("predict_map_points_element_ptr is nullptr");
  }

  if (matchItem.current_map_points_element_ptr == nullptr) {
    throw std::runtime_error("current_map_points_element_ptr is nullptr");
  }

  // std::cout << "match--score: " << matchedItem.score << std::endl;
  // std::cout << "match--(predict index, current index): "
  //           << "(" << matchedItem.predict_index << " , "
  //           << matchedItem.current_index << ")" << std::endl;
  auto const &p2cMinDistList{matchItem.predict_to_current_min_dist_list};
  auto const &c2pMinDistList{matchItem.current_to_predict_min_dist_list};
  auto &predictPointSet{matchItem.predict_map_points_element_ptr->points};
  auto const &currentPointSet{matchItem.current_map_points_element_ptr->points};

  fusion_result_ptr_->clear();

  auto const &overlapDomainPair{
      get_overlap_domain(p2cMinDistList, c2pMinDistList)};

  auto const &predictOverlapDomain{overlapDomainPair.first};
  auto const &currentOverlapDomain{overlapDomainPair.second};

  // merge predict&&current
  if (predictOverlapDomain.stop_index < predictOverlapDomain.start_index) {
    throw std::runtime_error(
        "predictFusionStartIndex larger than predictFusionStopIndex");
  }

  if (currentOverlapDomain.stop_index < currentOverlapDomain.start_index) {
    throw std::runtime_error(
        "currentFusionStartIndex larger than currentFusionStopIndex");
  }

  // fusion
  // concate head
  for (std::uint32_t i{0}; i < predictOverlapDomain.start_index; ++i) {
    fusion_result_ptr_->add(predictPointSet.at(i));
  }

  for (std::uint32_t i{0}; i < currentOverlapDomain.start_index; ++i) {
    auto const &p{currentPointSet.at(i)};

    if (is_greater(p.x, parameter.x_low_limit)) {
      fusion_result_ptr_->add(p);
    }
  }

  for (std::uint32_t i{predictOverlapDomain.start_index};
       i < predictOverlapDomain.stop_index + 1; ++i) {
    if (i >= p2cMinDistList.size()) {
      continue;
    }

    auto const &pointDistItem{p2cMinDistList.at(i)};
    auto const &predictPoint{predictPointSet.at(pointDistItem.src_index)};
    auto const &currentPoint{currentPointSet.at(pointDistItem.dst_index)};

    if (is_greater(currentPoint.x, parameter.x_low_limit) &&
        is_greater(parameter.threshold, pointDistItem.distance)) {
      auto const &fusionPoint{
          fusion_point(predictPoint, currentPoint,
                       parameter_.fusion_parameter.history_weight)};
      fusion_result_ptr_->add(fusionPoint);
    } else {
      fusion_result_ptr_->add(predictPoint);
    }
  }

  // concate tail
  if ((predictOverlapDomain.stop_index + 1) < (predictPointSet.size())) {
    for (std::uint32_t i{predictOverlapDomain.stop_index + 1};
         i < predictPointSet.size(); ++i) {
      fusion_result_ptr_->add(predictPointSet.at(i));
    }
  }

  if ((currentPointSet.size() - 1 > 0) &&
      (currentOverlapDomain.stop_index + 1 < currentPointSet.size())) {
    for (std::uint32_t i{currentOverlapDomain.stop_index + 1};
         i < currentPointSet.size(); ++i) {
      auto const &p{currentPointSet.at(i)};

      if (is_greater(p.x, parameter.x_low_limit)) {
        fusion_result_ptr_->add(p);
      }
    }
  }

  // update predict data
  predictPointSet.clear();
  for (std::uint32_t i{0}; i < fusion_result_ptr_->size(); ++i) {
    predictPointSet.add(fusion_result_ptr_->at(i));
  }
}

MapElementPoint MapFilterImpl::fusion_point(MapElementPoint const &p1,
                                            MapElementPoint const &p2,
                                            float weight) noexcept {
  MapElementPoint p;
  p.x = weight * p1.x + (1 - weight) * p2.x;
  p.y = weight * p1.y + (1 - weight) * p2.y;
  p.z = weight * p1.z + (1 - weight) * p2.z;
  p.score = weight * p1.score + (1 - weight) * p2.score;
  std::uint32_t const &rSize{sizeof(MapElementPoint::reserve) /
                             sizeof(MapElementPoint::type)};
  for (std::uint32_t i{0}; i < rSize; ++i) {
    p.reserve[i] = weight * p1.reserve[i] + (1 - weight) * p2.reserve[i];
  }
  return p;
}

void MapFilterImpl::operator()(Map &map,
                               Eigen::Affine3d const &pose) noexcept(false) {
  if (!IsInit()) {
    throw std::runtime_error("map filter is uninitialized");
  }
  Timer tP;
  auto historyPose{history_pose_};
  history_pose_ = pose;
  // predict road edge
  try {
    predict_map_element(history_map_ptr_->road_edge, parameter_, historyPose,
                        pose, *road_edge_predict_ptr_);
  } catch (const std::exception &e) {
    throw std::runtime_error("predict road edge error, " +
                             std::string(e.what()));
  }

  // predict lane divider
  try {
    predict_map_element(history_map_ptr_->lane_divider, parameter_, historyPose,
                        pose, *lane_divier_predict_ptr_);
  } catch (const std::exception &e) {
    throw std::runtime_error("predict lane divider error, " +
                             std::string(e.what()));
  }

  // predict center line
  try {
    predict_map_element(history_map_ptr_->center_line, parameter_, historyPose,
                        pose, *center_line_predict_ptr_);
  } catch (const std::exception &e) {
    throw std::runtime_error("predict center line error, " +
                             std::string(e.what()));
  }

  std::cout << "[Info] -----> map filter Predict Run time: "
            << autodrive::perception::to_green(
                   std::to_string(tP.elapsed() / (float)1000))
            << " ms, " << std::endl;

  Timer tF;
  // fusion road edge
  try {
    fusion_map_elment(parameter_, *road_edge_predict_ptr_, map.road_edge,
                      history_map_tmp_ptr_->road_edge);

    remove_repeat_map_element(parameter_, history_map_tmp_ptr_->road_edge,
                              history_map_ptr_->road_edge);
  } catch (const std::exception &e) {
    throw std::runtime_error("fusion road edge error, " +
                             std::string(e.what()));
  }

  try {
    fusion_map_elment(parameter_, *lane_divier_predict_ptr_, map.lane_divider,
                      history_map_tmp_ptr_->lane_divider);

    remove_repeat_map_element(parameter_, history_map_tmp_ptr_->lane_divider,
                              history_map_ptr_->lane_divider);
  } catch (const std::exception &e) {
    throw std::runtime_error("fusion lane divider error, " +
                             std::string(e.what()));
  }

  // auto &perceptionVisualizerDebug{
  //     autodrive::perception::PerceptionVisualizerDebug::GetInstance()};
  // perceptionVisualizerDebug.Update(map.timestamp,
  // history_map_ptr_->center_line,
  //                                  *center_line_predict_ptr_,
  //                                  map.center_line);
  try {
    fusion_map_elment(parameter_, *center_line_predict_ptr_, map.center_line,
                      history_map_tmp_ptr_->center_line);
    remove_repeat_map_element(parameter_, history_map_tmp_ptr_->center_line,
                              history_map_ptr_->center_line);
  } catch (const std::exception &e) {
    throw std::runtime_error("fusion center line error, " +
                             std::string(e.what()));
  }

  std::cout << "[Info] -----> map filter Fusion Run time: "
            << autodrive::perception::to_green(
                   std::to_string(tF.elapsed() / (float)1000))
            << " ms, " << std::endl;

  if (history_map_ptr_ != nullptr) {
    map.road_edge.clear();
    map.center_line.clear();
    map.lane_divider.clear();

    // std::cout << "history road edge list: " << std::endl;
    for (std::uint32_t i{0}; i < history_map_ptr_->road_edge.size(); ++i) {
      if (map.road_edge.full()) {
        break;
      }
      auto const &histRoadEdge{history_map_ptr_->road_edge.at(i)};
      if (histRoadEdge.life_time >= parameter_.lifetime_threshold) {
        // auto roadEdge{map.road_edge.end()};
        // // roadEdge.id = histRoadEdge.id;
        // roadEdge->category = histRoadEdge.category;
        // roadEdge->points = histRoadEdge.points;
        // map.road_edge.resize(map.road_edge.size() + 1);
        map.road_edge.add(histRoadEdge);
      }
      // std::cout << "(index, lifetime, points)" << i << ", "
      //           << histRoadEdge.life_time << ", "
      //           << histRoadEdge.points.GetNumberFilled() << std::endl;
    }
    // std::cout << "---------------------------" << std::endl << std::endl;

    for (std::uint32_t i{0}; i < history_map_ptr_->lane_divider.size(); ++i) {
      if (map.lane_divider.full()) {
        break;
      }

      auto const &histLaneDivier{history_map_ptr_->lane_divider.at(i)};
      if (histLaneDivier.life_time >= parameter_.lifetime_threshold) {
        // auto laneDivider{map.lane_divider.end()};
        // // laneDivider.id = histLaneDivier.id;
        // laneDivider->line_type = histLaneDivier.line_type;
        // laneDivider->color = histLaneDivier.color;
        // laneDivider->points = histLaneDivier.points;
        // map.lane_divider.resize(map.lane_divider.size() + 1);
        map.lane_divider.add(histLaneDivier);
      }
    }

    // std::cout << "history center line list: " << std::endl;
    for (std::uint32_t i{0}; i < history_map_ptr_->center_line.size(); ++i) {
      if (map.center_line.full()) {
        break;
      }

      auto const &histCenterLine{history_map_ptr_->center_line.at(i)};
      if (histCenterLine.life_time >= parameter_.lifetime_threshold) {
        // auto centerLine{map.center_line.end()};
        // // centerLine.id = histCenterLine.id;
        // centerLine->topo_point_ids = histCenterLine.topo_point_ids;
        // centerLine->points = histCenterLine.points;
        // map.center_line.resize(map.center_line.size() + 1);
        map.center_line.add(histCenterLine);
        // std::cout << "(index, lifetime, points)" << i << ", "
        //           << histCenterLine.life_time << ", "
        //           << histCenterLine.points.size() << std::endl;
      }
    }
    // std::cout << "---------------------------" << std::endl << std::endl;
  }
}

bool MapFilterImpl::is_point_in_roi(Point2f const &p,
                                    MapFilter::ROI const &roi) noexcept {
  auto xLowBoundary{is_greater(p.x, roi.top_left.x)};
  auto xUpBoundary{is_greater(roi.bottom_right.x, p.x)};

  auto yLowBoundary{is_greater(p.y, roi.bottom_right.y)};
  auto yUpBoundary{is_greater(roi.top_left.y, p.y)};

  auto xIn{xLowBoundary && xUpBoundary};
  auto yIn{yLowBoundary && yUpBoundary};

  auto isIn{xIn && yIn};

  if (isIn) {
    return true;
  } else {
    return false;
  }
}

void MapFilterImpl::calculate_chamfer_distance(
    MapElementPointSet const &points_1, MapElementPointSet const &points_2,
    MapFilter::ROI const &roi, ChamferDistanceResult &result) noexcept {
  auto &&chamferDistance{std::numeric_limits<double>::max()};
  auto const &mapPointsP1Number{points_1.size()};
  auto const &mapPoints2Number{points_2.size()};

  auto calcMinDistSum{[&](MapElementPointSet const &pointSet1,
                          MapElementPointSet const &pointSet2) {
    double &&minDistSum{0};
    double &&invalidDistValue{9999};
    auto const &p1SetNum{pointSet1.size()};
    auto const &p2SetNum{pointSet2.size()};
    std::vector<PointDistanceItem> minDistanceList;
    bool isP1InRoi{false};
    bool isP2InRoi{false};
    std::int32_t countTimes{0};
    std::int32_t searchSpec{5};
    std::int32_t searchStart{0};
    std::int32_t searchStop{static_cast<std::int32_t>(p2SetNum)};

    for (std::uint32_t i{0}; i < p1SetNum; ++i) {
      auto &&minDist{std::numeric_limits<float>::max()};
      std::uint32_t &&minIndex{0};
      auto const &p1{pointSet1.at(i)};
      if (is_point_in_roi(p1, roi)) {
        isP1InRoi = true;
      } else {
        isP1InRoi = false;
      }
      for (std::int32_t j{searchStart}; j < searchStop; ++j) {
        auto const &p2{pointSet2.at(j)};
        auto const &dist{calculate_point_distance(p1, p2)};
        if (is_greater(minDist, dist)) {
          minDist = dist;
          minIndex = j;
          if (is_point_in_roi(p2, roi)) {
            isP2InRoi = true;
          } else {
            isP2InRoi = false;
          }
        }
      }
      if (isP1InRoi && isP2InRoi) {
        minDistSum += minDist;
        ++countTimes;
      }

      minDistanceList.emplace_back(PointDistanceItem{i, minIndex, minDist});
      searchStart = minIndex - searchSpec;
      if (searchStart < 0) {
        searchStart = 0;
      }
      searchStop = minIndex + searchSpec;
      if (searchStop > p2SetNum) {
        searchStop = p2SetNum;
      }
    }

    if (countTimes > 0) {
      minDistSum = minDistSum / countTimes;
    } else {
      minDistSum = invalidDistValue;
    }
    return std::make_pair(minDistSum, minDistanceList);
  }};

  if (mapPointsP1Number > 0 && mapPoints2Number > 0) {
    auto minDistSumRet1{calcMinDistSum(points_1, points_2)};
    chamferDistance = minDistSumRet1.first;
    result.p1_p2_min_dist_list = minDistSumRet1.second;

    auto minDistSumRet2 = calcMinDistSum(points_2, points_1);
    chamferDistance += minDistSumRet2.first;
    result.p2_p1_min_dist_list = minDistSumRet2.second;

    auto const &minDist{std::min(minDistSumRet1.first, minDistSumRet2.first)};

    result.distance = clip<float>(minDist);
  }
}
}  // namespace perception
}  // namespace autodrive
